#include <Servo.h>

#define N_HEAD 1
#define I_HEAD_NECK 0

Servo servo_head[N_HEAD];
int ns_head[N_HEAD] = {
  28};
int pos_head[N_HEAD] = {
  90};

#define N_ARM 3
#define I_ARM_SHOULDER 0
#define I_ARM_BICEP 1
#define I_ARM_ELBOW 2

Servo servo_left_arm[N_ARM];
int ns_left_arm[N_ARM] = {
  46, 47, 48};
int pos_left_arm[N_ARM] = {
  90-60, 180, 180};

Servo servo_right_arm[N_ARM];
int ns_right_arm[N_ARM] = {
  20, 21, 22};
int pos_right_arm[N_ARM] = {
  90+60, 0, 0};

#define N_LEG 5
#define I_LEG_HIP1 0
#define I_LEG_HIP2 1
#define I_LEG_THIGH 2
#define I_LEG_KNEE 3
#define I_LEG_ANKLE 4

Servo servo_left_leg[N_LEG];
int ns_left_leg[N_LEG] = {
  41, 42, 43, 44, 45};
int pos_left_leg[N_LEG] = {
  90, 90, 90, 90, 90};

Servo servo_right_leg[N_LEG];
int ns_right_leg[N_LEG] = {
  15, 16, 17, 18, 19};
int pos_right_leg[N_LEG] = {
  90, 90, 90, 90, 90};

/// anim
unsigned long cur_anim_time; 
unsigned long anim_time;
int anim1_head[N_HEAD];
int anim2_head[N_HEAD];
int anim1_right_arm[N_ARM];
int anim2_right_arm[N_ARM];
int anim1_left_arm[N_ARM];
int anim2_left_arm[N_ARM];
int anim1_right_leg[N_LEG];
int anim2_right_leg[N_LEG];
int anim1_left_leg[N_LEG];
int anim2_left_leg[N_LEG];

int init_servos()
{
  for(int i=0; i<N_HEAD; i++)
  {
    servo_head[i].attach(ns_head[i]);
  }

  for(int i=0; i<N_ARM; i++)
  {
    servo_left_arm[i].attach(ns_left_arm[i]);
    servo_right_arm[i].attach(ns_right_arm[i]);
  }

  for(int i=0; i<N_LEG; i++)
  {
    servo_left_leg[i].attach(ns_left_leg[i]);
    servo_right_leg[i].attach(ns_right_leg[i]);
  }
}

void update_servos()
{
  for(int i=0; i<N_HEAD; i++)
  {
    servo_head[i].write(pos_head[i]);
  }

  for(int i=0; i<N_ARM; i++)
  {
    servo_left_arm[i].write(pos_left_arm[i]);
    servo_right_arm[i].write(pos_right_arm[i]);
  }

  for(int i=0; i<N_LEG; i++)
  {
    servo_left_leg[i].write(pos_left_leg[i]);
    servo_right_leg[i].write(pos_right_leg[i]);
  }
}

int anim_step_pos(int a, int b, float s)
{
  if(a==b) return a;
  
  float r = ((float)a*s) + ((float)b*(1.0f-s));
  return (int)r;
}

boolean do_anim(unsigned long slicetime)
{
  cur_anim_time += slicetime;
    
  // first step
  if(cur_anim_time==0)
  {
    for(int i=0; i<N_HEAD; i++)
    {
      pos_head[i] = anim1_head[i];
    }
  
    for(int i=0; i<N_ARM; i++)
    {
      pos_left_arm[i] = anim1_left_arm[i];
      pos_right_arm[i] = anim1_right_arm[i];
    }
  
    for(int i=0; i<N_LEG; i++)
    {
      pos_left_leg[i] = anim1_left_leg[i];
      pos_right_leg[i] = anim1_right_leg[i];
    }

    return false;
  }
  // end anim
  if(cur_anim_time>anim_time)
  {
    for(int i=0; i<N_HEAD; i++)
    {
      pos_head[i] = anim2_head[i];
    }
  
    for(int i=0; i<N_ARM; i++)
    {
      pos_left_arm[i] = anim2_left_arm[i];
      pos_right_arm[i] = anim2_right_arm[i];
    }
  
    for(int i=0; i<N_LEG; i++)
    {
      pos_left_leg[i] = anim2_left_leg[i];
      pos_right_leg[i] = anim2_right_leg[i];
    }
    
    return true;
  }
  else
  {
    float s = (float)(anim_time-cur_anim_time)/(float)anim_time;
    
    for(int i=0; i<N_HEAD; i++)
    {
      pos_head[i] = anim_step_pos(anim1_head[i], anim2_head[i], s);
    }
  
    for(int i=0; i<N_ARM; i++)
    {
      pos_left_arm[i] = anim_step_pos(anim1_left_arm[i], anim2_left_arm[i], s);
      pos_right_arm[i] = anim_step_pos(anim1_right_arm[i], anim2_right_arm[i], s);
    }
  
    for(int i=0; i<N_LEG; i++)
    {
      pos_left_leg[i] = anim_step_pos(anim1_left_leg[i], anim2_left_leg[i], s);
      pos_right_leg[i] = anim_step_pos(anim1_right_leg[i], anim2_right_leg[i], s);
    }

    delay(slicetime);
    return false;
  }
}

unsigned long g_time=0;
void setup()
{
  Serial.begin(9600);

  init_servos();
  g_time = millis();

  anim_right(0);
  update_servos();
}

boolean g_anim = false;
int g_what_anim = 0;
void loop()
{
  int val = 0;
  if(Serial.available())
    val = Serial.read();
  if(val=='a')
  {
    g_anim = !g_anim;
  }

  if(g_anim)
  {
    static int anim_step = 0;
    
    unsigned long cur_time = millis();
    unsigned long slicetime = cur_time-g_time;
    g_time = cur_time;
  
    if(val>0)
    {    
      if(val=='0')
      {
        anim_stand(0);
        g_what_anim = 0;
        anim_step=0;
        update_servos();
        delay(500);
      }
      else if(val=='1')
      {
        anim_pump_high(0);
        g_what_anim = 1;
        anim_step=0;
        update_servos();
        delay(500);
      }
      else if(val=='2')
      {
        anim_semipump_high(0);
        g_what_anim = 2;
        anim_step=0;
        update_servos();
        delay(500);
      }
    }

    if(do_anim(slicetime))
    {
      cur_anim_time = 0;
      anim_step++;
      if(g_what_anim == 0) // hulk
      {
        switch(anim_step)
        {
          case 0:
          case 6:
            anim_step = 0;
            anim_stand(1500);
          break;
          case 1:
            anim_stand2waitfor(750);
          break;
          case 2:
            anim_waitfor2hulk(150);
          break;
          case 3:
            anim_hulk(1500);
          break;
          case 4:
            anim_hulk2waitfor(250);
          break;          
          case 5:
            anim_waitfor2stand(250);
          break;
        }
      }
      else if(g_what_anim == 1) // sporting pumps
      {
        switch(anim_step)
        {
          case 0:
          case 3:
            anim_step = 0;
            anim_pump_high(500);
          break;
          case 1:
            anim_pump_high2low(750);
          break;
          case 2:
            anim_pump_low2high(750);
          break;
        }
      }
      else if(g_what_anim == 2) // sporting semipumps
      {
        switch(anim_step)
        {
          case 0:
          case 3:
            anim_step = 0;
            anim_semipump_high(500);
          break;
          case 1:
            anim_semipump_high2low(750);
          break;
          case 2:
            anim_semipump_low2high(750);
          break;
        }
      }
    }
    update_servos();
  }
  else
  {
    if(val>0)
    {    
      if(val=='0')
        set_position_right(pos_head, pos_left_arm, pos_left_leg, pos_right_arm, pos_right_leg);
      else if(val=='1')
        set_position_stand(pos_head, pos_left_arm, pos_left_leg, pos_right_arm, pos_right_leg);
      else if(val=='2')
        set_position_waitfor(pos_head, pos_left_arm, pos_left_leg, pos_right_arm, pos_right_leg);
      else if(val=='3')
        set_position_hulk(pos_head, pos_left_arm, pos_left_leg, pos_right_arm, pos_right_leg);
      else if(val=='4')
        set_position_pump_high(pos_head, pos_left_arm, pos_left_leg, pos_right_arm, pos_right_leg);
      else if(val=='5')
        set_position_pump_low(pos_head, pos_left_arm, pos_left_leg, pos_right_arm, pos_right_leg);
      else if(val=='7')
        set_position_semipump_high(pos_head, pos_left_arm, pos_left_leg, pos_right_arm, pos_right_leg);
      else if(val=='8')
        set_position_semipump_low(pos_head, pos_left_arm, pos_left_leg, pos_right_arm, pos_right_leg);
      else if(val=='9')
        set_position_plane(pos_head, pos_left_arm, pos_left_leg, pos_right_arm, pos_right_leg);
      update_servos();
      delay(500);
    }
  }
}

